/*
  speed_control
  2022-8-22 

  本程序旨在使用Da-Servo库控制连接在PCA9685舵机驱动板的舵机运行速度

*/

#include "DaServo.h"

#define startPos 0 //舵机往复运动起始位置 / Start position of servo move back and forth
#define endPos 180 //舵机往复运动结束位置 / End position of servo move back and forth

//建立舵机对象
DaServo myservo1(0);
DaServo myservo2(1);

// 自定义IIC引脚
static const uint8_t mSDA = 8; // esp32默认为8
static const uint8_t mSCL = 6; // esp32默认为9

void setup()
{

  Wire.setPins(mSDA, mSCL);

  myservo1.init();
  myservo2.init();
  
  myservo1.setTarget(endPos); //初始设定舵机运动目标位置 / Setup myservo1 initial target
  myservo2.setTarget(endPos); //初始设定舵机运动目标位置 / Setup myservo1 initial target
}

void loop()
{

  //控制舵机往复运动 / Servo moves back and forth
  if (myservo1.getPosition() == endPos)
  {
    myservo1.setServoSpeed(500); //设置舵机运行速度为500
    myservo1.setTarget(startPos);
  }
  else if (myservo1.getPosition() == startPos)
  {
    myservo1.setServoSpeed(200); //设置舵机运行速度为200
    myservo1.setTarget(endPos);
  }

    //舵机2往复运动 / myservo2 moves back and forth
  if (myservo2.getPosition() == endPos)
  {
    myservo2.setTarget(startPos);
  }
  else if (myservo2.getPosition() == startPos)
  {
    myservo2.setTarget(endPos);
  }

  //重要：servoUpdate函数用于更新舵运行状态，必须经常调用否则舵机无法正常运行!
  // IMPORTANT: servoUpdate MUST be called constantly or servo CANNOT work!
  myservo1.servoUpdate();
  myservo2.servoUpdate();
}
